
#include "stdio.h"
#include "stdlib.h"
#include "math.h"
 
double frand()
{
	return 2 * ((rand() / (double)RAND_MAX) - 0.5);//随机噪声
}
 
void Kalman(void)
{
	float x_last = 0;
	
	float p_last = 0.02;
	float Q = 0.018;
	float R = 0.542;
	float kg;
	
	float x_mid;
	float x_now;
	float p_mid;
	float p_now;
	float z_real = 0.56;
	float z_measure;
	float summerror_kalman = 0;
	float summerror_measure = 0;
	int i;
	x_last = z_real + frand()*0.03;
	x_mid = x_last;
	for (i = 0; i < 20;i++)
	{
		x_mid = x_last;
		p_mid = p_last + Q;
		kg = p_mid / (p_mid + R);
		z_measure = z_real + frand()*0.03;//测量值
		x_now = x_mid + kg*(z_measure - x_mid);//估计出的最有值
		p_now = (1 - kg)*p_mid;//最优值对应的协方差

		summerror_kalman += fabs(z_real - x_now);
		summerror_measure += fabs(z_real - z_measure);
		p_last = p_now;
		x_last = x_now;
	}
}




float kalmanFilter_A(float inData) 
{
  static float prevData=0; 
  static float p=10, q=0.0001, r=0.005, kGain=0;
    p = p+q; 
    kGain = p/(p+r);

    inData = prevData+(kGain*(inData-prevData)); 
    p = (1-kGain)*p;

    prevData = inData;

    return inData; 
}



